#include "MPU6050.h"

#define I2C_MASTER_SCL_IO           CONFIG_I2C_MASTER_SCL       /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           CONFIG_I2C_MASTER_SDA       /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              I2C_NUM_0                   /*!< I2C port number for master dev */
#define I2C_MASTER_FREQ_HZ          CONFIG_I2C_MASTER_FREQUENCY /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       1000

#define MPU9250_SENSOR_ADDR         0x68        /*!< Address of the MPU9250 sensor */
#define MPU9250_WHO_AM_I_REG_ADDR   0x75        /*!< Register addresses of the "who am I" register */
#define MPU9250_PWR_MGMT_1_REG_ADDR 0x6B        /*!< Register addresses of the power management register */
#define MPU9250_RESET_BIT           7

i2c_master_bus_handle_t bus_handle;
i2c_master_dev_handle_t dev_handle;

static void i2c_master_init(i2c_master_bus_handle_t *bus_handle, i2c_master_dev_handle_t *dev_handle)
{
    i2c_master_bus_config_t bus_config = {
        .i2c_port = I2C_MASTER_NUM,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
        .flags.enable_internal_pullup = true,
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_config, bus_handle));

    i2c_device_config_t dev_config = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address = MPU9250_SENSOR_ADDR,
        .scl_speed_hz = I2C_MASTER_FREQ_HZ,
    };
    ESP_ERROR_CHECK(i2c_master_bus_add_device(*bus_handle, &dev_config, dev_handle));
}

uint8_t MPU_Init()
{
	i2c_master_init(&bus_handle, &dev_handle);

	uint8_t res = 0;
	MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x80);
	vTaskDelay(100 / portTICK_RATE_MS);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0x00);
	vTaskDelay(100 / portTICK_RATE_MS);
	MPU_Set_Gyro_FSR(3);//3
	MPU_Set_Accel_FSR(0);
	MPU_Set_Rate(100);
	MPU_Write_Byte(MPU_INT_EN_REG,0X00);	//关闭所有中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);	//I2C主模式关闭
	MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);	//INT引脚低电平有效
	MPU_Read_Byte(MPU_DEVICE_ID_REG, &res);
	//if(res != MPU_ADDR)
	if(res != 0x70)
	{
		printf("WHO IS [%d]\n", res);
		return 1;
	}

	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);	//设置CLKSEL,PLL X轴为参考
	MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
	MPU_Set_Rate(100);						//设置采样率为50Hz
	return 0;
}

uint8_t MPU_Set_Gyro_FSR(uint8_t fsr)
{
	return MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);
}

uint8_t MPU_Set_Accel_FSR(uint8_t fsr)
{
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);
}

uint8_t MPU_Set_LPF(uint16_t lpf)
{
	uint8_t data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6;
	return MPU_Write_Byte(MPU_CFG_REG, data);
}

uint8_t MPU_Set_Rate(uint16_t rate)
{
	uint8_t data;
	if(rate > 1000)
		rate = 1000;
	if(rate < 4)
		rate = 4;
	data = 1000/rate - 1;
	data = MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);
	return MPU_Set_LPF(rate / 2); /*!< set low pass filter the half of the rate */
}

int16_t MPU_Get_Temperature()
{
	uint8_t buf[2];
	int16_t raw;
	float temp;
	if(MPU_Read_Len(MPU_TEMP_OUTH_REG, buf, 2) == 0)
		return 1;
	raw = ((uint16_t)(buf[1] << 8)) | buf[0];
	temp = 36.53 + ((double)raw/340);
	return temp*100;
}

uint8_t MPU_Get_Gyroscope(int16_t *gx, int16_t *gy, int16_t *gz)
{
	uint8_t buf[6], res;
	res = MPU_Read_Len(MPU_GYRO_XOUTH_REG, buf, 6);
	if(res == 0)
	{
		*gx=((uint16_t)buf[0]<<8)|buf[1];
		*gy=((uint16_t)buf[2]<<8)|buf[3];
		*gz=((uint16_t)buf[4]<<8)|buf[5];
	}
	return res;
}

uint8_t MPU_Get_Accelerometer(int16_t *ax, int16_t *ay, int16_t *az)
{
	uint8_t buf[6], res;
	res = MPU_Read_Len(MPU_ACCEL_XOUTH_REG, buf, 6);
	if(res == 0)
	{
		*ax=((uint16_t)buf[0]<<8)|buf[1];
		*ay=((uint16_t)buf[2]<<8)|buf[3];
		*az=((uint16_t)buf[4]<<8)|buf[5];
	}
	return res;
}


uint8_t MPU_Write_Byte(uint8_t reg, uint8_t data)
{
	esp_err_t error;
	
	uint8_t write_buf[2] = {reg, data};
	error = i2c_master_transmit(dev_handle, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
	if(error != ESP_OK) {
		return 1;
	}

	return 0;
}

uint8_t MPU_Write_Len(uint8_t reg, uint8_t *data, uint8_t len)
{
	esp_err_t error;
	
	// uint8_t write_buf[2] = {reg, 0};
	// error = i2c_master_transmit(dev_handle, write_buf, sizeof(write_buf), I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
	// if(error != ESP_OK) {
	// 	return 1;
	// }
	// error = i2c_master_transmit(dev_handle, data, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
	// if(error != ESP_OK) {
	// 	return 1;
	// }

	uint8_t *send_buffer = malloc(len + 1);
    send_buffer[0] = reg;
    for (int i = 0; i < len; i++) {
        send_buffer[i + 1] = data[i];
    }

	error = i2c_master_transmit(dev_handle, send_buffer, len + 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
	if(error != ESP_OK) {
		return 1;
	}

	free(send_buffer);

	return 0;
}

uint8_t MPU_Read_Byte(uint8_t reg, uint8_t *res)
{
	esp_err_t error;

	error = i2c_master_transmit_receive(dev_handle, &reg, 1, res, 1, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
	if(error != ESP_OK) {
		return 1;
	}

	return 0;
}

uint8_t MPU_Read_Len(uint8_t reg, uint8_t *buf, uint8_t len)
{
	esp_err_t error;

	error = i2c_master_transmit_receive(dev_handle, &reg, 1, buf, len, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
	if(error != ESP_OK) {
		return 1;
	}

	return 0;
}
